// Copyright 1998-2016 Glenn McIntosh
// licensed under the GNU General Public Licence version 3

// include files
#include "smooth.h"
#include "transform.h"
#include <functional>
#include <algorithm>
#include <numeric>
#include <cmath>
#include <exception>

namespace math
{
// Smooth a vector using an FIR filter
void fir_smooth(Vector &data, Vector &respns, bool wrap)
{
	// pad data
	int n = data.size();
	if (wrap)
	{
		int z = respns.size()/2;
		if ((n-1^n) < n || n < z)
		{
			// wrap around
			int nn;
			for (nn = 1; nn < n+z+z; nn <<= 1);
			data.insert(data.end(), nn-n, 0.0);
			int zz = z;
			while (zz > n)
			{
				std::copy(data.begin(), data.begin()+n, data.begin()+(n+z-zz));
				std::copy(data.begin(), data.begin()+n, data.end()-(n+z-zz));
				zz -= n;
			}
			std::copy(data.begin(), data.begin()+zz, data.begin()+(n+z-zz));
			std::copy(data.begin()+n-zz, data.begin()+n, data.end()-z);
		}
	}
	else
	{
		// extend end-point
		int z = respns.size()/2;
		int nn;
		for (nn = 1; nn < n+z+z; nn <<= 1);
		data.insert(data.end(), z, data[n-1]);
		data.insert(data.end(), nn-(n+z+z), 0.0);
		data.insert(data.end(), z, data[0]);
	}

	// filter data
	convolve(data, respns);

	// unpad data
	data.erase(data.begin()+n, data.end());
}

// Smooth a vector using a localized Gaussian average
void gaussian_smooth(const Vector &x, Vector &data, real b)
{
	// check arguments
	int n = data.size();
	if (n != static_cast<int>(x.size()))
		throw std::invalid_argument("axis arrays of different size");
	if (b < 0)
		throw std::out_of_range("negative standard deviation");

	// set ordering direction
	bool reverse = n > 0 && x[0] > x[n-1];
	if (reverse) b = -b;

	// evaluate localized Gaussian average at each point
	int iLeft = 0, iRight = 0;
	real r = b*0.52; // b*sqrt(2)*0.37
	Vector y = data;
	for (int i = 0; i < n; ++i)
	{
		// update left pointer
		real xLeft = x[i]-b*3;
		while (iLeft < n && (!reverse && x[iLeft] < xLeft || reverse && x[iLeft] > xLeft))

			++iLeft;
		// update right pointer
		real xRight = x[i]+b*3;
		while (iRight < n && (!reverse && x[iRight] <= xRight || reverse && x[iRight] >= xRight))
			++iRight;

		// accumulate
		real sumWeight = 0., sumData = 0.;
		for (int j = iLeft; j < iRight; ++j)
		{
			real xWeight = (x[i]-x[j])/r;
			xWeight = exp(-xWeight*xWeight);
			sumWeight += xWeight;
			sumData += xWeight*y[j];
		}
		data[i] = sumData/sumWeight;
	}
}
}
